{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import open3d as o3d\n",
    "import copy\n",
    "import numpy as np\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Multiway registration is the process of aligning multiple pieces of geometry in a global space. Typically, the input is a set of geometries (e.g., point clouds or RGBD images) \n",
    ". The output is a set of rigid transformations \n",
    ", so that the transformed point clouds \n",
    " are aligned in the global space.\n",
    "\n",
    "Open3D implements multiway registration via pose graph optimization. The backend implements the technique presented in [Choi2015].\n",
    "## Input\n",
    "The first part of the tutorial code reads three point clouds from files. The point clouds are downsampled and visualized together. They are misaligned."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def load_point_clouds(voxel_size=0.0):\n",
    "    pcds = []\n",
    "    demo_icp_pcds = o3d.data.DemoICPPointClouds()\n",
    "    for path in demo_icp_pcds.paths:\n",
    "        pcd = o3d.io.read_point_cloud(path)\n",
    "        pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)\n",
    "        pcds.append(pcd_down)\n",
    "    return pcds\n",
    "\n",
    "voxel_size = 0.02\n",
    "pcds_down = load_point_clouds(voxel_size)\n",
    "o3d.visualization.draw_geometries(pcds_down,\n",
    "                                  zoom=0.3412,\n",
    "                                  front=[0.4257, -0.2125, -0.8795],\n",
    "                                  lookat=[2.6172, 2.0475, 1.532],\n",
    "                                  up=[-0.0694, -0.9768, 0.2024])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Pose graph"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pairwise_registration(source, target):\n",
    "    print(\"Apply point-to-plane ICP\")\n",
    "    icp_coarse = o3d.pipelines.registration.registration_icp(\n",
    "        source, target, max_correspondence_distance_coarse, np.identity(4),\n",
    "        o3d.pipelines.registration.TransformationEstimationPointToPlane())\n",
    "    icp_fine = o3d.pipelines.registration.registration_icp(\n",
    "        source, target, max_correspondence_distance_fine,\n",
    "        icp_coarse.transformation,\n",
    "        o3d.pipelines.registration.TransformationEstimationPointToPlane())\n",
    "    transformation_icp = icp_fine.transformation\n",
    "    information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(\n",
    "        source, target, max_correspondence_distance_fine,\n",
    "        icp_fine.transformation)\n",
    "    return transformation_icp, information_icp\n",
    "\n",
    "\n",
    "def full_registration(pcds, max_correspondence_distance_coarse,\n",
    "                      max_correspondence_distance_fine):\n",
    "    pose_graph = o3d.pipelines.registration.PoseGraph()\n",
    "    odometry = np.identity(4)\n",
    "    pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))\n",
    "    n_pcds = len(pcds)\n",
    "    for source_id in range(n_pcds):\n",
    "        for target_id in range(source_id + 1, n_pcds):\n",
    "            transformation_icp, information_icp = pairwise_registration(\n",
    "                pcds[source_id], pcds[target_id])\n",
    "            print(\"Build o3d.pipelines.registration.PoseGraph\")\n",
    "            if target_id == source_id + 1:  # odometry case\n",
    "                odometry = np.dot(transformation_icp, odometry)\n",
    "                pose_graph.nodes.append(\n",
    "                    o3d.pipelines.registration.PoseGraphNode(\n",
    "                        np.linalg.inv(odometry)))\n",
    "                pose_graph.edges.append(\n",
    "                    o3d.pipelines.registration.PoseGraphEdge(source_id,\n",
    "                                                             target_id,\n",
    "                                                             transformation_icp,\n",
    "                                                             information_icp,\n",
    "                                                             uncertain=False))\n",
    "            else:  # loop closure case\n",
    "                pose_graph.edges.append(\n",
    "                    o3d.pipelines.registration.PoseGraphEdge(source_id,\n",
    "                                                             target_id,\n",
    "                                                             transformation_icp,\n",
    "                                                             information_icp,\n",
    "                                                             uncertain=True))\n",
    "    return pose_graph"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Full registration ...\n",
      "Apply point-to-plane ICP\n",
      "[Open3D DEBUG] ICP Iteration #0: Fitness 0.6258, RMSE 0.1566\n",
      "[Open3D DEBUG] Residual : 1.85e-02 (# of elements : 17029)\n",
      "[Open3D DEBUG] ICP Iteration #1: Fitness 0.6873, RMSE 0.1427\n",
      "[Open3D DEBUG] Residual : 1.34e-02 (# of elements : 18703)\n",
      "[Open3D DEBUG] ICP Iteration #2: Fitness 0.7258, RMSE 0.1364\n",
      "[Open3D DEBUG] Residual : 1.11e-02 (# of elements : 19751)\n",
      "[Open3D DEBUG] ICP Iteration #3: Fitness 0.7514, RMSE 0.1330\n",
      "[Open3D DEBUG] Residual : 1.02e-02 (# of elements : 20446)\n",
      "[Open3D DEBUG] ICP Iteration #4: Fitness 0.7743, RMSE 0.1296\n",
      "[Open3D DEBUG] Residual : 9.36e-03 (# of elements : 21070)\n",
      "[Open3D DEBUG] ICP Iteration #5: Fitness 0.7881, RMSE 0.1214\n",
      "[Open3D DEBUG] Residual : 7.82e-03 (# of elements : 21444)\n",
      "[Open3D DEBUG] ICP Iteration #6: Fitness 0.7959, RMSE 0.1142\n",
      "[Open3D DEBUG] Residual : 6.81e-03 (# of elements : 21657)\n",
      "[Open3D DEBUG] ICP Iteration #7: Fitness 0.8024, RMSE 0.1127\n",
      "[Open3D DEBUG] Residual : 6.56e-03 (# of elements : 21833)\n",
      "[Open3D DEBUG] ICP Iteration #8: Fitness 0.8026, RMSE 0.1108\n",
      "[Open3D DEBUG] Residual : 6.34e-03 (# of elements : 21840)\n",
      "[Open3D DEBUG] ICP Iteration #9: Fitness 0.7959, RMSE 0.1050\n",
      "[Open3D DEBUG] Residual : 5.79e-03 (# of elements : 21658)\n",
      "[Open3D DEBUG] ICP Iteration #10: Fitness 0.7816, RMSE 0.0914\n",
      "[Open3D DEBUG] Residual : 4.30e-03 (# of elements : 21268)\n",
      "[Open3D DEBUG] ICP Iteration #11: Fitness 0.7676, RMSE 0.0758\n",
      "[Open3D DEBUG] Residual : 2.52e-03 (# of elements : 20887)\n",
      "[Open3D DEBUG] ICP Iteration #12: Fitness 0.7573, RMSE 0.0682\n",
      "[Open3D DEBUG] Residual : 1.73e-03 (# of elements : 20606)\n",
      "[Open3D DEBUG] ICP Iteration #13: Fitness 0.7525, RMSE 0.0656\n",
      "[Open3D DEBUG] Residual : 1.48e-03 (# of elements : 20475)\n",
      "[Open3D DEBUG] ICP Iteration #14: Fitness 0.7500, RMSE 0.0645\n",
      "[Open3D DEBUG] Residual : 1.40e-03 (# of elements : 20407)\n",
      "[Open3D DEBUG] ICP Iteration #15: Fitness 0.7489, RMSE 0.0639\n",
      "[Open3D DEBUG] Residual : 1.36e-03 (# of elements : 20377)\n",
      "[Open3D DEBUG] ICP Iteration #16: Fitness 0.7482, RMSE 0.0635\n",
      "[Open3D DEBUG] Residual : 1.32e-03 (# of elements : 20358)\n",
      "[Open3D DEBUG] ICP Iteration #17: Fitness 0.7477, RMSE 0.0632\n",
      "[Open3D DEBUG] Residual : 1.31e-03 (# of elements : 20347)\n",
      "[Open3D DEBUG] ICP Iteration #18: Fitness 0.7476, RMSE 0.0631\n",
      "[Open3D DEBUG] Residual : 1.30e-03 (# of elements : 20342)\n",
      "[Open3D DEBUG] ICP Iteration #19: Fitness 0.7475, RMSE 0.0631\n",
      "[Open3D DEBUG] Residual : 1.29e-03 (# of elements : 20341)\n",
      "[Open3D DEBUG] ICP Iteration #20: Fitness 0.7475, RMSE 0.0630\n",
      "[Open3D DEBUG] Residual : 1.29e-03 (# of elements : 20339)\n",
      "[Open3D DEBUG] ICP Iteration #21: Fitness 0.7473, RMSE 0.0629\n",
      "[Open3D DEBUG] Residual : 1.29e-03 (# of elements : 20335)\n",
      "[Open3D DEBUG] ICP Iteration #22: Fitness 0.7473, RMSE 0.0629\n",
      "[Open3D DEBUG] Residual : 1.28e-03 (# of elements : 20334)\n",
      "[Open3D DEBUG] ICP Iteration #23: Fitness 0.7472, RMSE 0.0628\n",
      "[Open3D DEBUG] Residual : 1.28e-03 (# of elements : 20331)\n",
      "[Open3D DEBUG] ICP Iteration #24: Fitness 0.7471, RMSE 0.0627\n",
      "[Open3D DEBUG] Residual : 1.28e-03 (# of elements : 20329)\n",
      "[Open3D DEBUG] ICP Iteration #25: Fitness 0.7470, RMSE 0.0627\n",
      "[Open3D DEBUG] Residual : 1.27e-03 (# of elements : 20326)\n",
      "[Open3D DEBUG] ICP Iteration #26: Fitness 0.7469, RMSE 0.0626\n",
      "[Open3D DEBUG] Residual : 1.27e-03 (# of elements : 20325)\n",
      "[Open3D DEBUG] ICP Iteration #27: Fitness 0.7469, RMSE 0.0626\n",
      "[Open3D DEBUG] Residual : 1.27e-03 (# of elements : 20324)\n",
      "[Open3D DEBUG] ICP Iteration #28: Fitness 0.7469, RMSE 0.0626\n",
      "[Open3D DEBUG] Residual : 1.26e-03 (# of elements : 20323)\n",
      "[Open3D DEBUG] ICP Iteration #29: Fitness 0.7469, RMSE 0.0626\n",
      "[Open3D DEBUG] Residual : 1.26e-03 (# of elements : 20323)\n",
      "[Open3D DEBUG] ICP Iteration #0: Fitness 0.5852, RMSE 0.0139\n",
      "[Open3D DEBUG] Residual : 1.17e-04 (# of elements : 15924)\n",
      "[Open3D DEBUG] ICP Iteration #1: Fitness 0.6369, RMSE 0.0121\n",
      "[Open3D DEBUG] Residual : 7.15e-05 (# of elements : 17332)\n",
      "[Open3D DEBUG] ICP Iteration #2: Fitness 0.6395, RMSE 0.0102\n",
      "[Open3D DEBUG] Residual : 3.27e-05 (# of elements : 17402)\n",
      "[Open3D DEBUG] ICP Iteration #3: Fitness 0.6390, RMSE 0.0101\n",
      "[Open3D DEBUG] Residual : 3.13e-05 (# of elements : 17388)\n",
      "[Open3D DEBUG] ICP Iteration #4: Fitness 0.6392, RMSE 0.0101\n",
      "[Open3D DEBUG] Residual : 3.13e-05 (# of elements : 17392)\n",
      "[Open3D DEBUG] ICP Iteration #5: Fitness 0.6391, RMSE 0.0101\n",
      "[Open3D DEBUG] Residual : 3.13e-05 (# of elements : 17390)\n",
      "Build o3d.pipelines.registration.PoseGraph\n",
      "Apply point-to-plane ICP\n",
      "[Open3D DEBUG] ICP Iteration #0: Fitness 0.5669, RMSE 0.1655\n",
      "[Open3D DEBUG] Residual : 1.78e-02 (# of elements : 15427)\n",
      "[Open3D DEBUG] ICP Iteration #1: Fitness 0.6107, RMSE 0.1603\n",
      "[Open3D DEBUG] Residual : 1.72e-02 (# of elements : 16617)\n",
      "[Open3D DEBUG] ICP Iteration #2: Fitness 0.6706, RMSE 0.1484\n",
      "[Open3D DEBUG] Residual : 1.47e-02 (# of elements : 18249)\n",
      "[Open3D DEBUG] ICP Iteration #3: Fitness 0.7393, RMSE 0.1341\n",
      "[Open3D DEBUG] Residual : 9.62e-03 (# of elements : 20117)\n",
      "[Open3D DEBUG] ICP Iteration #4: Fitness 0.8061, RMSE 0.1313\n",
      "[Open3D DEBUG] Residual : 7.39e-03 (# of elements : 21934)\n",
      "[Open3D DEBUG] ICP Iteration #5: Fitness 0.8341, RMSE 0.1120\n",
      "[Open3D DEBUG] Residual : 5.13e-03 (# of elements : 22697)\n",
      "[Open3D DEBUG] ICP Iteration #6: Fitness 0.8784, RMSE 0.0993\n",
      "[Open3D DEBUG] Residual : 3.11e-03 (# of elements : 23901)\n",
      "[Open3D DEBUG] ICP Iteration #7: Fitness 0.9177, RMSE 0.0846\n",
      "[Open3D DEBUG] Residual : 1.47e-03 (# of elements : 24972)\n",
      "[Open3D DEBUG] ICP Iteration #8: Fitness 0.9376, RMSE 0.0795\n",
      "[Open3D DEBUG] Residual : 9.79e-04 (# of elements : 25512)\n",
      "[Open3D DEBUG] ICP Iteration #9: Fitness 0.9463, RMSE 0.0789\n",
      "[Open3D DEBUG] Residual : 9.53e-04 (# of elements : 25750)\n",
      "[Open3D DEBUG] ICP Iteration #10: Fitness 0.9491, RMSE 0.0788\n",
      "[Open3D DEBUG] Residual : 9.87e-04 (# of elements : 25827)\n",
      "[Open3D DEBUG] ICP Iteration #11: Fitness 0.9499, RMSE 0.0787\n",
      "[Open3D DEBUG] Residual : 1.00e-03 (# of elements : 25849)\n",
      "[Open3D DEBUG] ICP Iteration #12: Fitness 0.9503, RMSE 0.0787\n",
      "[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25858)\n",
      "[Open3D DEBUG] ICP Iteration #13: Fitness 0.9504, RMSE 0.0788\n",
      "[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25862)\n",
      "[Open3D DEBUG] ICP Iteration #14: Fitness 0.9505, RMSE 0.0788\n",
      "[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25863)\n",
      "[Open3D DEBUG] ICP Iteration #15: Fitness 0.9505, RMSE 0.0787\n",
      "[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25863)\n",
      "[Open3D DEBUG] ICP Iteration #16: Fitness 0.9505, RMSE 0.0788\n",
      "[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25863)\n",
      "[Open3D DEBUG] ICP Iteration #0: Fitness 0.6932, RMSE 0.0152\n",
      "[Open3D DEBUG] Residual : 1.42e-04 (# of elements : 18863)\n",
      "[Open3D DEBUG] ICP Iteration #1: Fitness 0.7057, RMSE 0.0106\n",
      "[Open3D DEBUG] Residual : 3.39e-05 (# of elements : 19203)\n",
      "[Open3D DEBUG] ICP Iteration #2: Fitness 0.7052, RMSE 0.0104\n",
      "[Open3D DEBUG] Residual : 3.06e-05 (# of elements : 19190)\n",
      "[Open3D DEBUG] ICP Iteration #3: Fitness 0.7054, RMSE 0.0104\n",
      "[Open3D DEBUG] Residual : 3.07e-05 (# of elements : 19195)\n",
      "[Open3D DEBUG] ICP Iteration #4: Fitness 0.7055, RMSE 0.0105\n",
      "[Open3D DEBUG] Residual : 3.07e-05 (# of elements : 19198)\n",
      "[Open3D DEBUG] ICP Iteration #5: Fitness 0.7055, RMSE 0.0105\n",
      "[Open3D DEBUG] Residual : 3.07e-05 (# of elements : 19197)\n",
      "Build o3d.pipelines.registration.PoseGraph\n",
      "Apply point-to-plane ICP\n",
      "[Open3D DEBUG] ICP Iteration #0: Fitness 0.7980, RMSE 0.1231\n",
      "[Open3D DEBUG] Residual : 1.23e-02 (# of elements : 15272)\n",
      "[Open3D DEBUG] ICP Iteration #1: Fitness 0.9232, RMSE 0.1132\n",
      "[Open3D DEBUG] Residual : 9.84e-03 (# of elements : 17668)\n",
      "[Open3D DEBUG] ICP Iteration #2: Fitness 0.9638, RMSE 0.0957\n",
      "[Open3D DEBUG] Residual : 4.76e-03 (# of elements : 18445)\n",
      "[Open3D DEBUG] ICP Iteration #3: Fitness 0.9648, RMSE 0.0865\n",
      "[Open3D DEBUG] Residual : 2.63e-03 (# of elements : 18464)\n",
      "[Open3D DEBUG] ICP Iteration #4: Fitness 0.9576, RMSE 0.0784\n",
      "[Open3D DEBUG] Residual : 1.34e-03 (# of elements : 18326)\n",
      "[Open3D DEBUG] ICP Iteration #5: Fitness 0.9506, RMSE 0.0755\n",
      "[Open3D DEBUG] Residual : 8.08e-04 (# of elements : 18191)\n",
      "[Open3D DEBUG] ICP Iteration #6: Fitness 0.9446, RMSE 0.0746\n",
      "[Open3D DEBUG] Residual : 5.87e-04 (# of elements : 18077)\n",
      "[Open3D DEBUG] ICP Iteration #7: Fitness 0.9400, RMSE 0.0740\n",
      "[Open3D DEBUG] Residual : 4.83e-04 (# of elements : 17989)\n",
      "[Open3D DEBUG] ICP Iteration #8: Fitness 0.9381, RMSE 0.0739\n",
      "[Open3D DEBUG] Residual : 4.41e-04 (# of elements : 17953)\n",
      "[Open3D DEBUG] ICP Iteration #9: Fitness 0.9373, RMSE 0.0737\n",
      "[Open3D DEBUG] Residual : 4.30e-04 (# of elements : 17937)\n",
      "[Open3D DEBUG] ICP Iteration #10: Fitness 0.9372, RMSE 0.0738\n",
      "[Open3D DEBUG] Residual : 4.29e-04 (# of elements : 17936)\n",
      "[Open3D DEBUG] ICP Iteration #11: Fitness 0.9372, RMSE 0.0739\n",
      "[Open3D DEBUG] Residual : 4.29e-04 (# of elements : 17936)\n",
      "[Open3D DEBUG] ICP Iteration #0: Fitness 0.7654, RMSE 0.0129\n",
      "[Open3D DEBUG] Residual : 9.10e-05 (# of elements : 14648)\n",
      "[Open3D DEBUG] ICP Iteration #1: Fitness 0.7605, RMSE 0.0109\n",
      "[Open3D DEBUG] Residual : 4.28e-05 (# of elements : 14553)\n",
      "[Open3D DEBUG] ICP Iteration #2: Fitness 0.7594, RMSE 0.0108\n",
      "[Open3D DEBUG] Residual : 4.23e-05 (# of elements : 14533)\n",
      "[Open3D DEBUG] ICP Iteration #3: Fitness 0.7592, RMSE 0.0108\n",
      "[Open3D DEBUG] Residual : 4.24e-05 (# of elements : 14528)\n",
      "Build o3d.pipelines.registration.PoseGraph\n"
     ]
    }
   ],
   "source": [
    "print(\"Full registration ...\")\n",
    "max_correspondence_distance_coarse = voxel_size * 15\n",
    "max_correspondence_distance_fine = voxel_size * 1.5\n",
    "with o3d.utility.VerbosityContextManager(\n",
    "        o3d.utility.VerbosityLevel.Debug) as cm:\n",
    "    pose_graph = full_registration(pcds_down,\n",
    "                                   max_correspondence_distance_coarse,\n",
    "                                   max_correspondence_distance_fine)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Optimizing PoseGraph ...\n",
      "[Open3D DEBUG] Validating PoseGraph - finished.\n",
      "[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 3 nodes and 3 edges.\n",
      "[Open3D DEBUG] Line process weight : 15.334500\n",
      "[Open3D DEBUG] [Initial     ] residual : 1.037615e+00, lambda : 2.960068e+00\n",
      "[Open3D DEBUG] [Iteration 00] residual : 2.034300e-01, valid edges : 1, time : 0.000 sec.\n",
      "[Open3D DEBUG] [Iteration 01] residual : 1.455356e-01, valid edges : 1, time : 0.000 sec.\n",
      "[Open3D DEBUG] Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)\n",
      "[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.000 sec.\n",
      "[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 3 nodes and 3 edges.\n",
      "[Open3D DEBUG] Line process weight : 15.334500\n",
      "[Open3D DEBUG] [Initial     ] residual : 1.455332e-01, lambda : 3.051329e+00\n",
      "[Open3D DEBUG] Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)\n",
      "[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.000 sec.\n",
      "[Open3D DEBUG] CompensateReferencePoseGraphNode : reference : 0\n"
     ]
    }
   ],
   "source": [
    "print(\"Optimizing PoseGraph ...\")\n",
    "option = o3d.pipelines.registration.GlobalOptimizationOption(\n",
    "    max_correspondence_distance=max_correspondence_distance_fine,\n",
    "    edge_prune_threshold=0.25,\n",
    "    reference_node=0)\n",
    "with o3d.utility.VerbosityContextManager(\n",
    "        o3d.utility.VerbosityLevel.Debug) as cm:\n",
    "    o3d.pipelines.registration.global_optimization(\n",
    "        pose_graph,\n",
    "        o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),\n",
    "        o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),\n",
    "        option)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualize optimization\n",
    "The transformed point clouds are listed and visualized using draw_geometries."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Transform points and display\n",
      "[[ 1.00000000e+00  1.34889997e-19  0.00000000e+00 -1.73472348e-18]\n",
      " [ 1.34678239e-19  1.00000000e+00  1.08420217e-19 -1.73472348e-18]\n",
      " [ 1.08420217e-19  0.00000000e+00  1.00000000e+00  0.00000000e+00]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "[[ 0.84016209 -0.14644296  0.52218973  0.34786264]\n",
      " [ 0.0061365   0.96535921  0.26085233 -0.39421642]\n",
      " [-0.54230065 -0.21595382  0.81195686  1.73016434]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "[[ 0.9627171  -0.07182071  0.26080179  0.37666506]\n",
      " [-0.00193493  0.96225765  0.27213318 -0.48961689]\n",
      " [-0.27050331 -0.2624919   0.92624293  1.29777557]\n",
      " [ 0.          0.          0.          1.        ]]\n"
     ]
    }
   ],
   "source": [
    "print(\"Transform points and display\")\n",
    "for point_id in range(len(pcds_down)):\n",
    "    print(pose_graph.nodes[point_id].pose)\n",
    "    pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)\n",
    "o3d.visualization.draw_geometries(pcds_down,\n",
    "                                  zoom=0.3412,\n",
    "                                  front=[0.4257, -0.2125, -0.8795],\n",
    "                                  lookat=[2.6172, 2.0475, 1.532],\n",
    "                                  up=[-0.0694, -0.9768, 0.2024])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Make a combined point cloud\n",
    "PointCloud has a convenience operator + that can merge two point clouds into a single one. In the code below, the points are uniformly resampled using voxel_down_sample after merging. This is recommended post-processing after merging point clouds since it can relieve duplicated or over-densified points."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "pcds = load_point_clouds(voxel_size)\n",
    "pcd_combined = o3d.geometry.PointCloud()\n",
    "for point_id in range(len(pcds)):\n",
    "    pcds[point_id].transform(pose_graph.nodes[point_id].pose)\n",
    "    pcd_combined += pcds[point_id]\n",
    "pcd_combined_down = pcd_combined.voxel_down_sample(voxel_size=voxel_size)\n",
    "o3d.io.write_point_cloud(\"multiway_registration.pcd\", pcd_combined_down)\n",
    "o3d.visualization.draw_geometries([pcd_combined_down],\n",
    "                                  zoom=0.3412,\n",
    "                                  front=[0.4257, -0.2125, -0.8795],\n",
    "                                  lookat=[2.6172, 2.0475, 1.532],\n",
    "                                  up=[-0.0694, -0.9768, 0.2024])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "notes",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
